#include "main.hpp"
#include "Thread.h"
#include "pick.hpp"
#include "tsp_solve.hpp"
char labels[15][10]=
{
"apple ", //0.fruit     /2/2
"banana", //1.fruit     /2/5
"bus   ", //2.vehicle   /3/5
"car   ", //3.vehicle   /3/4
"cat   ", //4.animal    /1/3
"cow   ", //5.animal    /1/4
"dog   ", //6.animal    /1/1
"durian", //7.fruit     /2/3
"grape ", //8.fruit     /2/4
"horse ", //9.animal    /1/2
"orange", //10.fruit    /2/1
"pig   ", //11.animal   /1/5
"plane ", //12.vehicle  /3/3
"ship  ", //13.vehicle  /3/2
"train "  //14.vehicle  /3/1
};


static void resultThreadEntry(void*) 
{
	using namespace artVcontact;
     
    for(;;)
    {
        if(rt_sem_take(recognition_ok_sem, RT_WAITING_FOREVER) == RT_EOK);
        {

       
            static uint8_t data_num=0;
            uint8_t sendstr[18];
            sprintf((char*)sendstr,"%03d.%03d %02d %02d %01d %01d\n",clock_s,clock_ms,report_coordx[data_num],report_coordy[data_num],labels_to_big_order(sorted_pictures[data_num]),labels_to_little_order(sorted_pictures[data_num]));
            rt_kprintf("%s:%s",labels[sorted_pictures[data_num]],sendstr);
            uart_putbuff(USART_1,sendstr,18);
            if(labels_to_big_order(sorted_pictures[data_num])==1)
            {
                SyncWritePosEx(ID, 3, Position_1, Speed_1, ACC_1);
                rt_thread_mdelay(700);
                Mag_Control(1,0);
                SyncWritePosEx(ID, 3, Position_W1, Speed_W1, ACC_W1);

            }
            else if(labels_to_big_order(sorted_pictures[data_num])==2)
            {
                SyncWritePosEx(ID, 3, Position_2, Speed_2, ACC_2);
                rt_thread_mdelay(700);
                Mag_Control(1,0);
                SyncWritePosEx(ID, 3, Position_W2, Speed_W2, ACC_W2);
            }
            else if(labels_to_big_order(sorted_pictures[data_num])==3)
            {
                SyncWritePosEx(ID, 3, Position_3, Speed_3, ACC_3);
                rt_thread_mdelay(700);
                Mag_Control(1,0);
                SyncWritePosEx(ID, 3, Position_W3, Speed_W3, ACC_W3);


            }
            data_num++;
        }

    }


}

rtthread::Thread resultThread(resultThreadEntry, NULL, 1024, 11, 1000, "resultThreadEntry");